#include "obstacle_detector/debug_display.h"
#include <pangolin/pangolin.h>

namespace obstacle_detector
{
DebugDisplay::DebugDisplay(std::string window_name):
window_name_(window_name)
{
	debug_filtered_points_=new int16_t[200000*4];
	debug_filtered_points_num_=0;
	debug_points_=new float[200000*5];
	debug_point_num_=0;
	debug_fitler_points_float_=new float[200000*4];
	debug_filtered_points_num_float_=0;
	predict_bbox_=new float [10000*8];
	predict_bbox_num_=0;
	thread_ = std::thread(std::bind(&DebugDisplay::debug_display,
		this));
}

DebugDisplay::~DebugDisplay()
{
	delete [] debug_points_;
}

void DebugDisplay::set_vehicle_param(const module_base::VehicleParam &vehicle_param)
{
	vehicle_param_=vehicle_param;
}

void DebugDisplay::set_state(const float *states)
{
	float vehicle_length=vehicle_param_.v_length;
	float vehicle_width=vehicle_param_.v_width;
	float back_edge_to_center=vehicle_param_.back_edge_to_center;
	float vehicle_bbox[8]={0,vehicle_width/2,
						   0,-1*vehicle_width/2,
						   0,-1*vehicle_width/2,
						   0,vehicle_width/2};
	// if (vehicle_type_=="QTRUCK")
	// {
		vehicle_bbox[0]=vehicle_length - back_edge_to_center;
		vehicle_bbox[2]=vehicle_length - back_edge_to_center;
		vehicle_bbox[4]=-1*back_edge_to_center;
		vehicle_bbox[6]=-1*back_edge_to_center;
	// }
	// else if (vehicle_type_=="IGV")
	// {
	// 	vehicle_bbox[0]=vehicle_length/2;
	// 	vehicle_bbox[2]=vehicle_length/2;
	// 	vehicle_bbox[4]=-1*vehicle_length/2;
	// 	vehicle_bbox[6]=-1*vehicle_length/2;
	// }

	std::vector<Eigen::MatrixXf> all_vechile_bbox;
	for (int i=1;i<51;i++)
	{
		// printf ("index: %d\n",i);
		Eigen::MatrixXf new_bbox(2,4);
		float yaw=-1*states[i*4+3];
		float x=states[i*4+0];
		float y=states[i*4+1];
		for (int j=0;j<4;j++)
		{
			new_bbox(0,j)=vehicle_bbox[j*2+0]*cos(yaw)+vehicle_bbox[j*2+1]*sin(yaw)+x;
			new_bbox(1,j)=-1*vehicle_bbox[j*2+0]*sin(yaw)+vehicle_bbox[j*2+1]*cos(yaw)+y;
		}
		all_vechile_bbox.push_back(new_bbox);
	}

	debug_vehicle_bbox_=all_vechile_bbox;
}

void DebugDisplay::set_showinfo(const float* points_int,int point_num)
{
	data_mutex_.lock();
	memcpy(debug_points_,points_int,sizeof(float)*point_num*5);
	debug_point_num_=point_num;
	data_mutex_.unlock();
}

void DebugDisplay::set_showinfo_float(const float* points,int point_num)
{
	data_mutex_.lock();
	memcpy(debug_fitler_points_float_,points,sizeof(float)*point_num*4);
	debug_filtered_points_num_float_=point_num;
	data_mutex_.unlock();
}

void DebugDisplay::set_showinfo(const int16_t* points_int,int point_num)
{
	data_mutex_.lock();
	memcpy(debug_filtered_points_,points_int,sizeof(int16_t)*point_num*4);
	debug_filtered_points_num_=point_num;
	data_mutex_.unlock();
}

void DebugDisplay::set_predict_bbox(const float *bbox,int bbox_num)
{
	data_mutex_.lock();
	memcpy(predict_bbox_,bbox,sizeof(float)*bbox_num*8);
	predict_bbox_num_=bbox_num;
	data_mutex_.unlock();
}

void DebugDisplay::show3Dbbox(std::vector<Object> all_bboxs)
{
	all_bboxs_=all_bboxs;
}

void DebugDisplay::show3Dbbox_compare(std::vector<Object> all_bboxs1,
	std::vector<Object> all_bboxs2)
{
	all_bboxs_global1_=all_bboxs1;
	all_bboxs_global2_=all_bboxs2;
}

void DebugDisplay::set_static_area(const std::vector<Eigen::MatrixXf> &areas)
{
	static_areas_=areas;
}

void DebugDisplay::set_road_area(const std::vector<Eigen::MatrixXf> &areas)
{
	road_areas_=areas;
}


void DebugDisplay::debug_display()
{
	pangolin::CreateWindowAndBind(window_name_,640,480);
	glEnable(GL_DEPTH_TEST);

	pangolin::OpenGlRenderState s_cam(
		pangolin::ProjectionMatrix(640,480,420,420,320,240,0.2,1000),
		pangolin::ModelViewLookAt(0,-50,10, 0,0,10, pangolin::AxisZ)
	);

	pangolin::Handler3D handler(s_cam);
	pangolin::View& d_cam = pangolin::CreateDisplay()
		.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
		.SetHandler(&handler);


	while( !pangolin::ShouldQuit())
	{
		
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		d_cam.Activate(s_cam);

		glPointSize(2);
		glBegin(GL_POINTS);
		glColor3f(0,1,0);
		data_mutex_.lock();
		for (int i=0;i<debug_point_num_;i++)
		{
			// glVertex3f(debug_points_[i*5+0],debug_points_[i*5+1],debug_points_[i*5+2]);
			glVertex3f(debug_points_[i*5+0],debug_points_[i*5+1],0);
		}
		for (int i=0;i<debug_filtered_points_num_;i++)
		{
			glVertex3f(debug_filtered_points_[i*4+0]*1.0/256,debug_filtered_points_[i*4+1]*1.0/256,0);
		}
		for (int i=0;i<debug_filtered_points_num_float_;i++)
		{
			glVertex3f(debug_fitler_points_float_[i*4+0],debug_fitler_points_float_[i*4+1],0);
		}
		data_mutex_.unlock();
	
		glEnd();

		glLineWidth(1);
		glBegin ( GL_LINES );
		glColor3f(1,0,0);
		glVertex3f(0,0,0);
		glVertex3f(0,0,10);
		glColor3f(0,0,1);
		glVertex3f(0,0,0);
		glVertex3f(70,0,0);
		glColor3f(0,1,0);
		glVertex3f(0,0,0);
		glVertex3f(0,10,0);


		glColor3f(1,0,0);
		for (int i=0;i<all_bboxs_.size();i++)
		{
			for (int j=0;j<4;j++)
			{
				glVertex3f(all_bboxs_[i].bbox(0,j),all_bboxs_[i].bbox(1,j),0);
				glVertex3f(all_bboxs_[i].bbox(0,(j+1)%4),all_bboxs_[i].bbox(1,(j+1)%4),0);

				// glVertex3f(all_bboxs_[i].bbox(0,j),all_bboxs_[i].bbox(1,j),all_bboxs_[i].bbox(2,j));
				// glVertex3f(all_bboxs_[i].bbox(0,(j+1)%4),all_bboxs_[i].bbox(1,(j+1)%4),all_bboxs_[i].bbox(2,(j+1)%4));
				// glVertex3f(all_bboxs_[i](0,j),all_bboxs_[i](1,j),all_bboxs_[i](2,j));
				// glVertex3f(all_bboxs_[i](0,(j+1)%4),all_bboxs_[i](1,(j+1)%4),all_bboxs_[i](2,(j+1)%4));
				// glVertex3f(all_bboxs_[i](0,j),all_bboxs_[i](1,j),all_bboxs_[i](2,j));
				// glVertex3f(all_bboxs_[i](0,j+4),all_bboxs_[i](1,j+4),all_bboxs_[i](2,j+4));
				// glVertex3f(all_bboxs_[i](0,j+4),all_bboxs_[i](1,j+4),all_bboxs_[i](2,j+4));
				// glVertex3f(all_bboxs_[i](0,(j+1)%4+4),all_bboxs_[i](1,(j+1)%4+4),all_bboxs_[i](2,(j+1)%4+4));
			}
		}


		glColor3f(1,0,0);
		for (int i=0;i<all_bboxs_global1_.size();i++)
		{
			for (int j=0;j<4;j++)
			{
				glVertex3f(all_bboxs_global1_[i].global_bbox(0,j),all_bboxs_global1_[i].global_bbox(1,j),0);
				glVertex3f(all_bboxs_global1_[i].global_bbox(0,(j+1)%4),all_bboxs_global1_[i].global_bbox(1,(j+1)%4),0);
			}
			glColor3f(0,1,1);
			for (int j=0;j<all_bboxs_global1_[i].predict_locs.cols()-1;j++)
			{
				glVertex3f(all_bboxs_global1_[i].predict_locs(0,j),all_bboxs_global1_[i].predict_locs(1,j),0);
				glVertex3f(all_bboxs_global1_[i].predict_locs(0,j+1),all_bboxs_global1_[i].predict_locs(1,j+1),0);
			}
			// printf ("%d history_locs size: %d\n",i,all_bboxs_global2_[i].history_locs.size()-1);
			glColor3f(1,0,0);
			if (all_bboxs_global1_[i].history_locs.size()>1)
			{
				for (int j=0;j<(all_bboxs_global1_[i].history_locs.size()-1);j++)
				{
					glVertex3f(all_bboxs_global1_[i].history_locs[j](0),all_bboxs_global1_[i].history_locs[j](1),0);
					glVertex3f(all_bboxs_global1_[i].history_locs[j+1](0),all_bboxs_global1_[i].history_locs[j+1](1),0);
				}
			}

			glColor3f(1,0,1);
			for (int j=0;j<4;j++)
			{
				glVertex3f(all_bboxs_global1_[i].min_bbox(0,j),all_bboxs_global1_[i].min_bbox(1,j),0);
				glVertex3f(all_bboxs_global1_[i].min_bbox(0,(j+1)%4),all_bboxs_global1_[i].min_bbox(1,(j+1)%4),0);
			}
		}
		for (int i=0;i<all_bboxs_global2_.size();i++)
		{
			glColor3f(0,1,1);
			for (int j=0;j<4;j++)
			{
				glVertex3f(all_bboxs_global2_[i].global_bbox(0,j),all_bboxs_global2_[i].global_bbox(1,j),0);
				glVertex3f(all_bboxs_global2_[i].global_bbox(0,(j+1)%4),all_bboxs_global2_[i].global_bbox(1,(j+1)%4),0);
			}
			// Eigen::Vector2f center=all_bboxs_global2_[i].global_center;
			// Eigen::Vector2f center_v=center+all_bboxs_global2_[i].velocity;
			// glVertex3f(center[0],center[1],0);
			// glVertex3f(center_v[0],center_v[1],0);

		}

		glColor3f(1,1,0);
		for (int i=0;i<static_areas_.size();i++)
		{
			for (int j=0;j<4;j++)
			{
				glVertex3f(static_areas_[i](0,j),static_areas_[i](1,j),0);
				glVertex3f(static_areas_[i](0,(j+1)%4),static_areas_[i](1,(j+1)%4),0);
			}
		}

		glColor3f(0,1,1);
		for (int i=0;i<road_areas_.size();i++)
		{
			for (int j=0;j<4;j++)
			{
				glVertex3f(road_areas_[i](0,j),road_areas_[i](1,j),0);
				glVertex3f(road_areas_[i](0,(j+1)%4),road_areas_[i](1,(j+1)%4),0);
			}
		}

		glColor3f(0,0,1);
		for (int i=0;i<debug_vehicle_bbox_.size();i++)
		{
			for (int j=0;j<4;j++)
			{
				glVertex3f(debug_vehicle_bbox_[i](0,j),debug_vehicle_bbox_[i](1,j),0);
				glVertex3f(debug_vehicle_bbox_[i](0,(j+1)%4),debug_vehicle_bbox_[i](1,(j+1)%4),0);
			}
		}

		glColor3f(1,0,1);
		for (int i=0;i<predict_bbox_num_;i++)
		{
			for (int j=0;j<4;j++)
			{
				glVertex3f(predict_bbox_[i*8+j*2+0],predict_bbox_[i*8+j*2+1],0);
				glVertex3f(predict_bbox_[i*8+(j+1)%4*2+0],predict_bbox_[i*8+(j+1)%4*2+1],0);
			}
		}

		glEnd();
		pangolin::FinishFrame();
	}
	pangolin::DestroyWindow(window_name_);
}

}